#include "mult_lidar/barrel_detector.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include <cmath>
#include <geometry_msgs/msg/point_stamped.hpp>

namespace mult_lidar{

    BarrelDetector::BarrelDetector(){

    }

    void BarrelDetector::setParameters(rclcpp::Node* node) {
        node->get_parameter("barrel.ring_height_threshold", params_.ring_height_threshold);
        node->get_parameter("barrel.barrel_radius", params_.barrel_radius);
        node->get_parameter("barrel.radius_filter_margin", params_.radius_filter_margin);
        node->get_parameter("barrel.search_start_z", params_.search_start_z);
        node->get_parameter("barrel.search_end_z", params_.search_end_z);
        node->get_parameter("barrel.search_step", params_.search_step);
        node->get_parameter("barrel.min_points_in_slice", params_.min_points_in_slice);
        node->get_parameter("barrel.min_points_after_filter", params_.min_points_after_filter);
        node->get_parameter("barrel.max_radius_error", params_.max_radius_error);

    }

    bool BarrelDetector::detectCenter(PointCloud cloud,
                                    geometry_msgs::msg::PointStamped& center_point){
        
        if(cloud->empty()){
            return false;
        }

        //1. 提取圆环部分
        float detected_z = NAN;
        auto ring_cloud = extractRingCloud(cloud, detected_z);
        if(std::isnan(detected_z)){
            return false;
        }

        if(static_cast<int>(ring_cloud->size())< params_.min_points_in_slice){
            return false;
        } 

        //2. 半径过滤
        auto filtered_ring_cloud = filterByRadius(ring_cloud);
        if(static_cast<int>(filtered_ring_cloud->size()) < params_.min_points_after_filter){
            filtered_ring_cloud = ring_cloud;//回退到未过滤的点云
        }

        //3. 圆拟合
        float center_x, center_y;
        if(!fitCircle(filtered_ring_cloud, center_x, center_y)){
            return false;
        }

        //4. 验证结果
        if(!validateFit(filtered_ring_cloud, center_x, center_y)){
            //验证失败但仍继续
        }


        //5. 设置中心点
        center_point.point.x = center_x;
        center_point.point.y = center_y;
        center_point.point.z = detected_z;
        center_point.header.frame_id = "map";


        //桶中心检测日志
        RCLCPP_INFO(rclcpp::get_logger("xxxxbarrel_detectorxxxx"), 
                "Barrel center detected: (%.3f, %.3f, %.3f)", 
                center_x, center_y, detected_z);


        //6. 发布圆环点云用于调试
        if(ring_cloud_pub_){
            sensor_msgs::msg::PointCloud2 ring_msg;
            pcl::toROSMsg(*filtered_ring_cloud, ring_msg);
            ring_msg.header.frame_id = "map";
            ring_cloud_pub_->publish(ring_msg);
        }

        return true;
    
    }

    BarrelDetector::PointCloud BarrelDetector::extractRingCloud(const PointCloud& cloud,
                                                                float& detected_z){

        auto ring_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
        detected_z = NAN;

        for(float current_z = params_.search_start_z;
            current_z > params_.search_end_z;
            current_z -= params_.search_step){

            ring_cloud->clear();

            //提取当前高度切片
            for(const auto& point : *cloud){
                if(point.z >= current_z &&
                   point.z <= current_z + params_.ring_height_threshold){
                    ring_cloud->push_back(point);
                }
            }

            //检查点云密度
            if(static_cast<int>(ring_cloud->size()) > params_.min_points_in_slice){
                detected_z = current_z;
                break;
            }

        }

        return ring_cloud;
    }

    BarrelDetector::PointCloud BarrelDetector::filterByRadius(const PointCloud& ring_cloud){

        auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();

        //计算点云中心作为初始参考点
        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(*ring_cloud, centroid);

        //过滤点：保留在半径范围内的点
        float min_radius = params_.barrel_radius - params_.radius_filter_margin;
        float max_radius = params_.barrel_radius + params_.radius_filter_margin;

        for(const auto& point : *ring_cloud) {
            float distance = std::sqrt(std::pow(point.x - centroid[0], 2) +
                                    std::pow(point.y - centroid[1], 2));
            if(distance >= min_radius && distance <= max_radius) {
                filtered_cloud->push_back(point);
            }
        }

        return filtered_cloud;
    }


    bool BarrelDetector::fitCircle (const PointCloud& cloud, float& center_x, float& center_y) {
        if(cloud->size() < 3) {
            return false;
        }

        //将点投影至XY平面
        Eigen::MatrixXf points(cloud->size(), 2);
        for(size_t i = 0; i < cloud->size(); ++i) {
            points(i, 0) = cloud->points[i].x;
            points(i, 1) = cloud->points[i].y;
        }

        //构建线性方程
        Eigen::VectorXf x = points.col(0);
        Eigen::VectorXf y = points.col(1);
        Eigen::VectorXf x2 = x.array().square();
        Eigen::VectorXf y2 = y.array().square();

        Eigen::MatrixXf A(cloud->size(), 3);
        A.col(0) = 2 * x;
        A.col(1) = 2 * y;
        A.col(2) = Eigen::VectorXf::Ones(cloud->size());

        Eigen::VectorXf b = x2 + y2;

        //求解线性方程组
        Eigen::Vector3f circle_params = A.colPivHouseholderQr().solve(b);
        center_x = circle_params[0];
        center_y = circle_params[1];

        return true;

    }


    bool BarrelDetector::validateFit(const PointCloud& cloud,
                                    float center_x, float center_y) {

        float mean_distance = 0.0;
        for(const auto& point : *cloud) {
            float distance = std::sqrt(std::pow(point.x - center_x, 2) +
                                      std::pow(point.y - center_y, 2));
            mean_distance += distance;
        }

        mean_distance /= cloud->size();

        float radius_error = std::abs(mean_distance - params_.barrel_radius);
        if(radius_error > params_.max_radius_error) {
            return false;
        }

        return true;
        
    }

}